
#define WIN32_LEAN_AND_MEAN
#include "stdafx.h"

#include <windows.h>
#include <d3d9.h>
#include <d3dx9.h>
#include <amp.h>
#include <amp_math.h>
#include "resource.h"
#include <ctime>
#include <cstring>
#include <string>
#include "Raptor.h"
#include <iostream>

// include the Direct3D Library file
#pragma comment (lib, "d3d9.lib")
#pragma comment (lib, "d3dx9d.lib")

using namespace concurrency;
//-----------------------------------------------------------------------------
// GLOBALS
//-----------------------------------------------------------------------------
HWND              g_hWnd = NULL;
LPDIRECT3D9       g_pD3D = NULL;
LPDIRECT3DDEVICE9 g_pd3dDevice = NULL;

unsigned int WINDOW_WIDTH = 1240;
unsigned int WINDOW_HEIGHT = 860;

Raptor * planner;

//-----------------------------------------------------------------------------
// PROTOTYPES
//-----------------------------------------------------------------------------
int WINAPI WinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance,
	LPSTR lpCmdLine, int nCmdShow);
LRESULT CALLBACK WindowProc(HWND hwnd, UINT uMsg, WPARAM wParam, LPARAM lParam);
void init(void);
void shutDown(void);
void render(void);

void motion_planning_scene();
void motion_planning_init();
void motion_planning_finalize();
void motion_planning_execute();

//-----------------------------------------------------------------------------
// Name: WinMain()
// Desc: The application's entry point
//-----------------------------------------------------------------------------
int WINAPI WinMain(HINSTANCE hInstance,
	HINSTANCE hPrevInstance,
	LPSTR     lpCmdLine,
	int       nCmdShow)
{
	WNDCLASSEX winClass;
	MSG        uMsg;

	memset(&uMsg, 0, sizeof(uMsg));

	winClass.lpszClassName = L"MY_WINDOWS_CLASS";
	winClass.cbSize = sizeof(WNDCLASSEX);
	winClass.style = CS_HREDRAW | CS_VREDRAW;
	winClass.lpfnWndProc = WindowProc;
	winClass.hInstance = hInstance;
	winClass.hIcon = LoadIcon(hInstance, (LPCTSTR)IDI_DIRECTX_ICON);
	winClass.hIconSm = LoadIcon(hInstance, (LPCTSTR)IDI_DIRECTX_ICON);
	winClass.hCursor = LoadCursor(NULL, IDC_ARROW);
	winClass.hbrBackground = (HBRUSH)GetStockObject(BLACK_BRUSH);
	winClass.lpszMenuName = NULL;
	winClass.cbClsExtra = 0;
	winClass.cbWndExtra = 0;

	if (!RegisterClassEx(&winClass))
		return E_FAIL;

	g_hWnd = CreateWindowEx(NULL, L"MY_WINDOWS_CLASS",
		L"Direct3D (DX9) - Initialization",
		WS_OVERLAPPEDWINDOW | WS_VISIBLE,
		0, 0, WINDOW_WIDTH, WINDOW_HEIGHT, NULL, NULL, hInstance, NULL);

	if (g_hWnd == NULL)
		return E_FAIL;

	ShowWindow(g_hWnd, nCmdShow);
	UpdateWindow(g_hWnd);

	init();

	while (uMsg.message != WM_QUIT)
	{
		if (PeekMessage(&uMsg, NULL, 0, 0, PM_REMOVE))
		{
			TranslateMessage(&uMsg);
			DispatchMessage(&uMsg);
		}
		else
			render();
	}

	shutDown();

	UnregisterClass(L"MY_WINDOWS_CLASS", winClass.hInstance);

	return uMsg.wParam;
}

//-----------------------------------------------------------------------------
// Name: WindowProc()
// Desc: The window's message handler
//-----------------------------------------------------------------------------
LRESULT CALLBACK WindowProc(HWND   hWnd,
	UINT   msg,
	WPARAM wParam,
	LPARAM lParam)
{
	switch (msg)
	{
	case WM_KEYDOWN:
	{
		switch (wParam)
		{
		case VK_ESCAPE:
			PostQuitMessage(0);
			break;
		}
	}
		break;

	case WM_CLOSE:
	{
		PostQuitMessage(0);
	}

	case WM_DESTROY:
	{
		PostQuitMessage(0);
	}
		break;

	default:
	{
		return DefWindowProc(hWnd, msg, wParam, lParam);
	}
		break;
	}

	return 0;
}



//-----------------------------------------------------------------------------
// Name: Log
// Desc: 
//-----------------------------------------------------------------------------

void Log(std::wstring txt)
{
	txt += L"\n";

	HANDLE myConsoleHandle = GetStdHandle(STD_OUTPUT_HANDLE);
	LPDWORD cCharsWritten = 0;
	WriteConsoleW(myConsoleHandle, txt.c_str(), txt.size(), cCharsWritten, NULL);
	// http://www.halcyon.com/~ast/dload/guicon.htm
	// check out that tutorial
}

void Log(std::string txt)
{
	std::wstring txtw(txt.begin(), txt.end());
	Log(txtw);
}

void default_gpu_properties() {
	accelerator default_acc;

	Log("");
	Log("dedicated_memory = " + std::to_string(default_acc.dedicated_memory));
	Log(default_acc.supports_cpu_shared_memory ? "CPU shared memory: true" : "CPU shared memory: false");
	Log(default_acc.supports_double_precision ?	"double precision: true" : "double precision: false");
	Log(default_acc.supports_limited_double_precision ? "limited double precision: true" : "limited double precision: false");
	Log("");
}

//-----------------------------------------------------------------------------
// Name: init()
// Desc: 
//-----------------------------------------------------------------------------

void init(void)
{
	g_pD3D = Direct3DCreate9(D3D_SDK_VERSION);

	if (g_pD3D == NULL)
	{
		// TO DO: Respond to failure of Direct3DCreate8
		return;
	}

	D3DDISPLAYMODE d3ddm;

	if (FAILED(g_pD3D->GetAdapterDisplayMode(D3DADAPTER_DEFAULT, &d3ddm)))
	{
		// TO DO: Respond to failure of GetAdapterDisplayMode
		return;
	}

	HRESULT hr;

	if (FAILED(hr = g_pD3D->CheckDeviceFormat(D3DADAPTER_DEFAULT, D3DDEVTYPE_HAL,
		d3ddm.Format, D3DUSAGE_DEPTHSTENCIL,
		D3DRTYPE_SURFACE, D3DFMT_D16)))
	{
		if (hr == D3DERR_NOTAVAILABLE)
			// POTENTIAL PROBLEM: We need at least a 16-bit z-buffer!
			return;
	}

	//
	// Do we support hardware vertex processing? if so, use it. 
	// If not, downgrade to software.
	//

	D3DCAPS9 d3dCaps;

	if (FAILED(g_pD3D->GetDeviceCaps(D3DADAPTER_DEFAULT,
		D3DDEVTYPE_HAL, &d3dCaps)))
	{
		// TO DO: Respond to failure of GetDeviceCaps
		return;
	}

	DWORD dwBehaviorFlags = 0;

	if (d3dCaps.VertexProcessingCaps != 0)
		dwBehaviorFlags |= D3DCREATE_HARDWARE_VERTEXPROCESSING;
	else
		dwBehaviorFlags |= D3DCREATE_SOFTWARE_VERTEXPROCESSING;

	//
	// Everything checks out - create a simple, windowed device.
	//

	D3DPRESENT_PARAMETERS d3dpp;
	memset(&d3dpp, 0, sizeof(d3dpp));

	d3dpp.BackBufferFormat = d3ddm.Format;
	d3dpp.SwapEffect = D3DSWAPEFFECT_DISCARD;
	d3dpp.Windowed = TRUE;
	d3dpp.EnableAutoDepthStencil = TRUE;
	d3dpp.AutoDepthStencilFormat = D3DFMT_D16;
	d3dpp.PresentationInterval = D3DPRESENT_INTERVAL_IMMEDIATE;

	if (FAILED(g_pD3D->CreateDevice(D3DADAPTER_DEFAULT, D3DDEVTYPE_HAL, g_hWnd,
		dwBehaviorFlags, &d3dpp, &g_pd3dDevice)))
	{
		// TO DO: Respond to failure of CreateDevice
		return;
	}


	//--------------------------------------------------------------------------
	AllocConsole();

	HWND hWnd = GetConsoleWindow();
	MoveWindow(hWnd, WINDOW_WIDTH, 0, 800, WINDOW_HEIGHT, TRUE);
	Log("Console created");

	default_gpu_properties();

	motion_planning_init();
}

//-----------------------------------------------------------------------------
// Name: shutDown()
// Desc: 
//-----------------------------------------------------------------------------
void shutDown(void)
{
	if (g_pd3dDevice != NULL)
		g_pd3dDevice->Release();

	if (g_pD3D != NULL)
		g_pD3D->Release();

	motion_planning_finalize();
}



//-----------------------------------------------------------------------------
// Name: DrawTextString
// 
//-----------------------------------------------------------------------------
ID3DXFont *g_font = NULL;
RECT fontPosition;
inline void DrawTextString(int x, int y, DWORD color, wchar_t const * str)
{
	if (g_font == NULL)
	{
		D3DXCreateFont(g_pd3dDevice,     //D3D Device
			15,               //Font height
			6,                //Font width
			FW_NORMAL,        //Font Weight
			1,                //MipLevels
			false,            //Italic
			DEFAULT_CHARSET,  //CharSet
			OUT_DEFAULT_PRECIS, //OutputPrecision
			ANTIALIASED_QUALITY, //Quality
			DEFAULT_PITCH | FF_DONTCARE,//PitchAndFamily
			L"Arial",          //pFacename,
			&g_font);         //ppFont


	}

	fontPosition.bottom = 0;
	fontPosition.left = x;
	fontPosition.right = 0;
	fontPosition.top = y;

	g_font->DrawText(NULL,        //pSprite
		str,//L"This is the first D3D program",  //pString
		-1,          //Count
		&fontPosition,  //pRect
		DT_LEFT | DT_NOCLIP,//Format,
		color); //Color
}

//-----------------------------------------------------------------------------
// Name: render()
// Desc: 
//-----------------------------------------------------------------------------
clock_t t_previous_rendering;
void render(void)
{
	g_pd3dDevice->Clear(0, NULL, D3DCLEAR_TARGET | D3DCLEAR_ZBUFFER,
		D3DCOLOR_COLORVALUE(1.0f, 1.0f, 1.0f, 1.0f), 1.0f, 0);

	g_pd3dDevice->BeginScene();

	// Render geometry here...

	motion_planning_scene();

	// ------------------------------


	float fps = (float)CLOCKS_PER_SEC / (clock() - t_previous_rendering);
	std::wstring text(L"fps = ");
	text += std::to_wstring(fps);

	unsigned int text_x_position = 10;
	unsigned int text_y_position = 10;
	unsigned int text_y_position_inc = 15;
	DWORD text_color = 0X88ff0000;

	DrawTextString(text_x_position, text_y_position, text_color, text.c_str());
	text_y_position += text_y_position_inc;

	if (planner != NULL)
	{
		DrawTextString(text_x_position, text_y_position, text_color, L"------  Raptor msg  ------");
		text_y_position += text_y_position_inc;
		std::vector<std::string> msg = planner->msg_output;
		for (unsigned int i = 0; i < msg.size(); ++i)
		{
			std::wstring txtw(msg.at(i).begin(), msg.at(i).end());
			DrawTextString(text_x_position, text_y_position, text_color, txtw.c_str());
			text_y_position += text_y_position_inc;
		}

	}
	// ------------------------------


	g_pd3dDevice->EndScene();
	g_pd3dDevice->Present(NULL, NULL, NULL, NULL);

	t_previous_rendering = clock();
	Sleep(100);//sleep 16 milliseconds
}

//==================================================
//=========== Motion Planning Context ==============
//
//================== Variables =====================

LPD3DXLINE line;
bool motion_planning_initialized = false;

double** state_list = NULL;
unsigned int n_state = 1024*128;

//================== Functions =====================
void motion_planning_init()
{
	D3DXCreateLine(g_pd3dDevice, &line);
	planner = new Raptor();

	motion_planning_initialized = true;
	Log("Motion Planning initiated.");
	
	motion_planning_execute();
}

void motion_planning_execute()
{
	Log("Motion Planning start.");
	
	planner->motion_planning_start();
}


void motion_planning_finalize()
{
	if (planner != NULL)
		delete planner;
}

struct CUSTOMVERTEX
{
	FLOAT x, y, z, rhw;    // from the D3DFVF_XYZRHW flag
	DWORD color;    // from the D3DFVF_DIFFUSE flag
};

unsigned int CANVAS_X = 410;
unsigned int CANVAS_Y = 10;
unsigned int CANVAS_WIDTH = 800;
unsigned int CANVAS_HEIGHT = 800;
D3DCOLOR CANVAS_COLOR = D3DCOLOR_XRGB(200, 200, 200);

CUSTOMVERTEX vertices_canvas[] =
{
	{ CANVAS_X, CANVAS_Y, 0, 1.0f, CANVAS_COLOR },
	{ CANVAS_X + CANVAS_WIDTH, CANVAS_Y, 0, 1.0f, CANVAS_COLOR },
	{ CANVAS_X, CANVAS_Y + CANVAS_HEIGHT, 0, 1.0f, CANVAS_COLOR },
	{ CANVAS_X + CANVAS_WIDTH, CANVAS_Y + CANVAS_HEIGHT, 0, 1.0f, CANVAS_COLOR }
};

#define CUSTOMFVF (D3DFVF_XYZRHW | D3DFVF_DIFFUSE)
LPDIRECT3DVERTEXBUFFER9 v_buffer = NULL;
LPDIRECT3DVERTEXBUFFER9 buffer_state = NULL;
unsigned int capacity_state_buffer = 6 * 1024 * 128;
CUSTOMVERTEX *vertices_state = NULL;
VOID* pVoid;    // the void* we were talking about

double range_x_min = -10;
double range_x_max = 10;
double range_y_min = -10;
double range_y_max = 10;

float state_to_screen_x(double q_x)
{
	return (float)((q_x - range_x_min) / (range_x_max - range_x_min) * CANVAS_WIDTH + CANVAS_X);
}

float state_to_screen_y(double q_y)
{
	return (float)((q_y - range_y_min) / (range_y_max - range_y_min) * CANVAS_HEIGHT + CANVAS_Y);
}

void motion_planning_clear_canvas()
{
	if (v_buffer == NULL)
	{
		g_pd3dDevice->CreateVertexBuffer(4 * sizeof(CUSTOMVERTEX),
			0,
			CUSTOMFVF,
			D3DPOOL_MANAGED,
			&v_buffer,
			NULL);

		v_buffer->Lock(0, 0, (void**)&pVoid, 0);    // locks v_buffer, the buffer we made earlier
		memcpy(pVoid, vertices_canvas, sizeof(vertices_canvas));    // copy vertices to the vertex buffer
		v_buffer->Unlock();    // unlock v_buffer
	}
	
	g_pd3dDevice->SetFVF(CUSTOMFVF);
	g_pd3dDevice->SetStreamSource(0, v_buffer, 0, sizeof(CUSTOMVERTEX));

	g_pd3dDevice->DrawPrimitive(D3DPT_TRIANGLESTRIP, 0, 2);

	D3DXVECTOR2 lines_x[] = {
		D3DXVECTOR2(state_to_screen_x(-10), state_to_screen_y(0)),
		D3DXVECTOR2(state_to_screen_x(10), state_to_screen_y(0))};
	D3DXVECTOR2 lines_y[] = {
		D3DXVECTOR2(state_to_screen_x(0), state_to_screen_y(10)),
		D3DXVECTOR2(state_to_screen_x(0), state_to_screen_y(-10)) };

	line->Begin();
	line->Draw(lines_x, 2, D3DCOLOR_ARGB(255, 10, 10, 10));
	line->End();
	line->Begin();
	line->Draw(lines_y, 2, D3DCOLOR_ARGB(255, 10, 10, 10));
	line->End();
}

D3DCOLOR edge_color = D3DCOLOR_XRGB(20, 100, 150);
void edge_to_vertex_buffer(double* q_i, double* q_parent, unsigned int index)
{
	float r = 1.25;

	double p1_x = state_to_screen_x(q_i[0]);
	double p1_y = state_to_screen_y(q_i[1]);
	double p0_x = state_to_screen_x(q_parent[0]);
	double p0_y = state_to_screen_y(q_parent[1]);
	double v_x = p1_x - p0_x;
	double v_y = p1_y - p0_y;

	double v_l = sqrt(v_x*v_x + v_y*v_y);
	double r_x = -v_y / v_l * r;
	double r_y = v_x / v_l * r;

	double p2_x = p0_x + r_x;
	double p2_y = p0_y + r_y;
	double p3_x = p0_x - r_x;
	double p3_y = p0_y - r_y;


	unsigned int n0 = index * 3;
	vertices_state[n0].x = p1_x;
	vertices_state[n0].y = p1_y;
	vertices_state[n0].z = 0;
	vertices_state[n0].rhw = 1;
	vertices_state[n0].color = edge_color;

	unsigned int n1 = n0 + 1;
	vertices_state[n1].x = p2_x;
	vertices_state[n1].y = p2_y;
	vertices_state[n1].z = 0;
	vertices_state[n1].rhw = 1;
	vertices_state[n1].color = edge_color;

	unsigned int n2 = n0 + 2;
	vertices_state[n2].x = p3_x;
	vertices_state[n2].y = p3_y;
	vertices_state[n2].z = 0;
	vertices_state[n2].rhw = 1;
	vertices_state[n2].color = edge_color;

}

D3DCOLOR state_color = D3DCOLOR_XRGB(20, 20, 200);
void state_to_vertex_buffer(double x, double y, unsigned int index_in_buffer)
{
	float r = 2.5;

	unsigned int n0 = index_in_buffer * 6;
	vertices_state[n0].x = state_to_screen_x(x) - r;
	vertices_state[n0].y = state_to_screen_y(y) - r;
	vertices_state[n0].z = 0;
	vertices_state[n0].rhw = 1;
	vertices_state[n0].color = state_color;

	unsigned int n1 = n0 + 1;
	vertices_state[n1].x = state_to_screen_x(x) + r;
	vertices_state[n1].y = state_to_screen_y(y) + r;
	vertices_state[n1].z = 0;
	vertices_state[n1].rhw = 1;
	vertices_state[n1].color = state_color;
	
	unsigned int n2 = n0 + 2;
	vertices_state[n2].x = state_to_screen_x(x) - r;
	vertices_state[n2].y = state_to_screen_y(y) + r;
	vertices_state[n2].z = 0;
	vertices_state[n2].rhw = 1;
	vertices_state[n2].color = state_color;

	unsigned int n3 = n0 + 3;
	vertices_state[n3].x = state_to_screen_x(x) - r;
	vertices_state[n3].y = state_to_screen_y(y) - r;
	vertices_state[n3].z = 0;
	vertices_state[n3].rhw = 1;
	vertices_state[n3].color = state_color;

	unsigned int n4 = n0 + 4;
	vertices_state[n4].x = state_to_screen_x(x) + r;
	vertices_state[n4].y = state_to_screen_y(y) - r;
	vertices_state[n4].z = 0;
	vertices_state[n4].rhw = 1;
	vertices_state[n4].color = state_color;

	unsigned int n5 = n0 + 5;
	vertices_state[n5].x = state_to_screen_x(x) + r;
	vertices_state[n5].y = state_to_screen_y(y) + r;
	vertices_state[n5].z = 0;
	vertices_state[n5].rhw = 1;
	vertices_state[n5].color = state_color;
}

std::vector<void*>* train = NULL;
void motion_planning_draw_states()
{
	if (buffer_state == NULL)
	{
		g_pd3dDevice->CreateVertexBuffer(capacity_state_buffer * sizeof(CUSTOMVERTEX),
			0,
			CUSTOMFVF,
			D3DPOOL_MANAGED,
			&buffer_state,
			NULL);
	}
	
	if (vertices_state == NULL)
	{
		vertices_state = new CUSTOMVERTEX[capacity_state_buffer];
	}

	if (n_state * 6 > capacity_state_buffer)
		throw new std::exception("too many states");

	std::vector<void*>* train_tmp = planner->get_probed_elements();
	if (train_tmp != train)
	{
		train = train_tmp;
		unsigned int i_max = train->size();
		if (i_max > n_state)
			i_max = n_state;
		for (unsigned int i = 0; i < i_max; ++i)
		{
			State* x_i = (State*)train->at(i);
			double* q_i = x_i->get_q();

			double* q_parent = x_i->get_parent_q();
			if (q_parent == NULL)
				continue;
			
			edge_to_vertex_buffer(q_i, q_parent, i);
			//state_to_vertex_buffer(q_i[0], q_i[1], i);
		}

		buffer_state->Lock(0, 0, (void**)&pVoid, 0);
		memcpy(pVoid, vertices_state, sizeof(CUSTOMVERTEX) * n_state * 6);
		buffer_state->Unlock();
	}

	g_pd3dDevice->SetFVF(CUSTOMFVF);
	g_pd3dDevice->SetStreamSource(0, buffer_state, 0, sizeof(CUSTOMVERTEX));

	HRESULT result = g_pd3dDevice->DrawPrimitive(D3DPT_TRIANGLELIST, 0, n_state * 2);
}


void motion_planning_scene()
{
	if (motion_planning_initialized == false)
		return;

	motion_planning_clear_canvas();
	
	// Draw motion planning scence

	motion_planning_draw_states();
}